classdef IMUOut2InTest_Cont < matlab.unittest.TestCase
    
    properties (TestParameter)
        NonLinearSFErr = struct('false', false, 'true', true);
        OuterIterStepNum = {4};
        InnerIterStepNum = {4};
    end
    
    methods (Test)
        function vldIMUOut2In_cont(testCase, NonLinearSFErr, OuterIterStepNum, InnerIterStepNum)
            % 验证连续形式的传感器误差计算
            disp(['NonLinearSFErr = ' int2str(NonLinearSFErr)]);
            
            %% 参数设置
            nSamp = 10000;
            fB = randi([-20, 20], [3 nSamp]) * 9.8; % 20g
            omegaIBB = randi([-100, 100], [3 nSamp]) / 180 * pi; % 100°/s
            omegaIBBRate = randi([-200, 200], [3 nSamp]) / 180 * pi; % 200°/s^2
            IMUPar = genrandimupar(nSamp, NonLinearSFErr);
            
            %% 计算加入误差的输出值
            [dfB, fTildeS] = accerror_cont(fB, IMUPar.acc, omegaIBB, omegaIBBRate, false);
            fTildeOutS_fromErr = NaN(size(fB));
            for i=1:length(fB)
                fTildeOutS_fromErr(:, i) = IMUPar.acc.KScal0(:, i).*((IMUPar.acc.PS0B(:, :, i)')*(dfB(:, i)+fB(:, i)));
            end
            fTildeOutS_direct = IMUPar.acc.KScal0.*fTildeS;
            disp('使用误差量计算与直接计算的fTildeOutS之相对差绝对值最大值');
            disp(max(abs(relatdiff(fTildeOutS_fromErr, fTildeOutS_direct)), [], 2)');
            
            [domegaIBB, omegaTildeIBS] = gyroerror_cont(omegaIBB, IMUPar.gyro, fB, false);
            omegaTildeIBOutS_fromErr = NaN(size(omegaIBB));
            for i=1:length(omegaIBB)
                omegaTildeIBOutS_fromErr(:, i) = IMUPar.gyro.KScal0(:, i).*((IMUPar.gyro.PS0B(:, :, i)')*(domegaIBB(:, i)+omegaIBB(:, i)));
            end
            omegaTildeIBOutS_direct = IMUPar.gyro.KScal0.*omegaTildeIBS;
            disp('使用误差量计算与直接计算的omegaTildeIBOutS之相对差绝对值最大值');
            disp(max(abs(relatdiff(omegaTildeIBOutS_fromErr, omegaTildeIBOutS_direct)), [], 2)'); % TODO: 陀螺误差为0时该值较大，需要验证
            disp('~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~');
            
            %% 计算输入值
            % 函数验证 线性化法
            innerIterAlgoType = int8(0);
            [fB_linear, omegaIBB_linear, accTriadJfCond1, gyroTriadJfCond1] = imuout2in_cont(fTildeOutS_direct, omegaTildeIBOutS_direct, IMUPar, ...
                omegaIBBRate, innerIterAlgoType*ones(2, 1, 'int8'), OuterIterStepNum, InnerIterStepNum); %#ok<ASGLU>
            fBErr_linear = (fB_linear-fB)' / 9.8 * 1e6;
            disp('线性化法fB误差绝对值最大值（μg）'); % 应为可以忽略的量级
            disp(max(abs(fBErr_linear), [], 1));
            disp('线性化法fB误差绝对值平均值（μg）'); % 应为可以忽略的量级
            disp(mean(abs(fBErr_linear), 1));
            omegaIBBErr_linear = (omegaIBB_linear-omegaIBB)' / pi * 180 * 3600;
            disp('线性化法omegaIBB误差绝对值最大值（°/h）'); % 应为可以忽略的量级
            disp(max(abs(omegaIBBErr_linear), [], 1));
            disp('线性化法omegaIBB误差绝对值平均值（°/h）'); % 应为可以忽略的量级
            disp(mean(abs(omegaIBBErr_linear), 1));
            if ~NonLinearSFErr
                testCase.verifyLessThanOrEqual(max(abs(fBErr_linear)), 8e-4);
                testCase.verifyLessThanOrEqual(mean(abs(fBErr_linear)), 2e-7);
                testCase.verifyLessThanOrEqual(max(abs(omegaIBBErr_linear)), 2e-6);
                testCase.verifyLessThanOrEqual(mean(abs(omegaIBBErr_linear)), 1e-9);
            end
            disp('~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~');
            
            % 函数验证 牛顿迭代法
            innerIterAlgoType = int8(1);
            [fB_newton, omegaIBB_newton, accTriadJfCond2, gyroTriadJfCond2] = imuout2in_cont(fTildeOutS_direct, omegaTildeIBOutS_direct, IMUPar, ...
                omegaIBBRate, innerIterAlgoType*ones(2, 1, 'int8'), OuterIterStepNum, InnerIterStepNum); %#ok<ASGLU>
            fBErr_newton = (fB_newton-fB)' / 9.8 * 1e6;
            disp('牛顿迭代法fB误差绝对值最大值（μg）'); % 应为可以忽略的量级
            disp(max(abs(fBErr_newton), [], 1));
            disp('牛顿迭代法fB误差绝对值平均值（μg）'); % 应为可以忽略的量级
            disp(mean(abs(fBErr_newton), 1));
            omegaIBBErr_newton = (omegaIBB_newton-omegaIBB)' / pi * 180 * 3600;
            disp('牛顿迭代法omegaIBB误差绝对值最大值（°/h）'); % 应为可以忽略的量级
            disp(max(abs(omegaIBBErr_newton), [], 1));
            disp('牛顿迭代法omegaIBB误差绝对值平均值（°/h）'); % 应为可以忽略的量级
            disp(mean(abs(omegaIBBErr_newton), 1));
            testCase.verifyLessThanOrEqual(max(abs(fBErr_newton)), 8e-4);
            testCase.verifyLessThanOrEqual(mean(abs(fBErr_newton)), 2e-7);
            testCase.verifyLessThanOrEqual(max(abs(omegaIBBErr_newton)), 2e-6);
            testCase.verifyLessThanOrEqual(mean(abs(omegaIBBErr_newton)), 1e-9);
            disp('~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~');
        end
    end
end